#include "powerhouse.h"

void init() {
    OSCCON = 0x32; //Set up for 1Mhz oporation
    TRISE = 0x00; // E0 input (DAT)  outputs E1 (CLK)  E2 (load)

    // PWM Configuration
    PR2 = 0xFF;
    T3CON = 0x03;
    TMR2 = 0x00;
    T2CON = 0xFF; //turn on and max out prescaler

    // Configure ADC Conversion Rate and Acquisition Time
    ADCON0 = 0x11; // Sample on AN4; turn ADC on
    ADCON1 = 0x08; // special trigger from CCP5; VREF+ is FVR; VREF- is VSS
    ADCON2 = 0xB6; // Right justified; 16 TAD acq time; FOSC/64

    // Configure ports
    // port directions: 1=input, 0=output

    // one analog input on port A for ADC
    TRISA = 0x20;
    ANSELA = 0x20;

    // digital inputs on port B bits 2, 3 for bumpers;
    // 5 for MOSI; 6 for SPI Clk
    TRISB = 0x37;
    ANSELB = 0x00;

    // PWM on port C
    TRISC = 0x00;
    ANSELC = 0x00;

    // SPI configuration
    SSP2STAT = 0b01000000;
    SSP2CON1 = 0b00100101;

    // Turn off DAC to use RA2
    VREFCON0 = 0xB0; // Use FVR = 4.096 V
    VREFCON1 = 0x00;

    // Configure pull-ups on port B
    INTCON2 = 0b00000000; // enable weak pull-up resistors
    WPUB = 0b00110000; // weak pull-ups on pins 4 and 5

    // Pre-feed SPI buffer with bumper info
    SSP2BUF = getBumpers();

    // http://ww1.microchip.com/downloads/en/DeviceDoc/51702a.pdf
    TMR0 = 0; //Clear the TMR0 register
    /*Configure Timer0 as follows:
    - Use the internal instruction clock
     as the source to the module
    - Assign the Prescaler to the Watchdog
     Timer so that TMR0 increments at a 1:1
     ratio with the internal instruction clock*/
    T0CON = 0B10001000; // Also, 16-bit timer
    T0IE = 1; //enable TMR0 overflow interrupts
    GIE = 1; //enable Global interrupts

    // Flash LEDs (diagnostic)
    setRedOn();
    setBlueOn();
    setGreenOn();
    setLowBattOn();

    Delay10KTCYx(10);

    setRedOff();
    setBlueOff();
    setGreenOff();
    setLowBattOff();
}

void idEx(unsigned char code) {
    unsigned char speed,
                  servoDebug = 0;

    if (code >= 0x40 && (code & 0x20) == 0x20) { // motor command and debugging
        servoDebug = 1;
        code -= 0x20; // motor command with debugging = motor command + 0x20
    }

    switch (code) {
        // LED cases
        case 0x10:
            setRedOff();
            break;
        case 0x11:
            setRedOn();
            break;
        case 0x20:
            setGreenOff();
            break;
        case 0x21:
            setGreenOn();
            break;
        case 0x30:
            setBlueOff();
            break;
        case 0x31:
            setBlueOn();
            break;

        // Servo cases
        case 0x40:
            rightServoStop();
            break;
        case 0x41:
            rightServoGo();
            break;
        case 0x42:
            rightServoReverse();
            break;
        case 0x43:
            rightServoGoVar();
            break;
        case 0x44:
            rightServoReverseVar();
            break;
        case 0x45:
            rightServoRun(SPI());
            break;

        case 0x80:
            leftServoStop();
            break;
        case 0x81:
            leftServoGo();
            break;
        case 0x82:
            leftServoReverse();
            break;
        case 0x83:
            leftServoGoVar();
            break;
        case 0x84:
            leftServoReverseVar();
            break;
        case 0x85:
            leftServoRun(SPI());
            break;

        case 0xc0:
            bothServosStop();
            break;
        case 0xc1:
            bothServosGo();
            break;
        case 0xc2:
            bothServosReverse();
            break;
        case 0xc3:
            bothServosGoVar();
            break;
        case 0xc4:
            bothServosReverseVar();
            break;

        case 0xc5:
            spinRight();
            break;
        case 0xc6:
            spinLeft();
            break;
        case 0xc7:
            turnRight();
            break;
        case 0xc8:
            turnLeft();
            break;
        case 0xc9:
            spinRightVar();
            break;
        case 0xca:
            spinLeftVar();
            break;
        case 0xcb:
            turnRightVar();
            break;
        case 0xcc:
            turnLeftVar();
            break;
    }

    if (servoDebug) {
        SSP2BUF = rightServoRead();
        SPI();
        SSP2BUF = leftServoRead();
        SPI();
    }
}
